module numeric_deriv; 
use numeric_grid;
implicit none;
logical, save :: FUNC_DRV_TRACE_ENTRY = .TRUE.;
logical, save :: CACL_DRV_P9_TRACE = .TRUE.;

!from p24 of Bickley MathGaz 25 (1941)
! NOTE!!! Be careful, fortran mtrx is by-column
real(R14), dimension(5,9), save :: DRV_P9 = RESHAPE(& 
    (/-109584D0, 322560D0, -564480D0,  752640D0, -705600D0 &
    ,   -5040D0, -64224D0,  141120D0, -141120D0,  117600D0 &
    ,     720D0, -11520D0,  -38304D0,   80640D0,  -50400D0 &
    ,    -240D0,   2880D0,  -20160D0,  -18144D0,   50400D0 &
    ,     144D0,  -1536D0,    8064D0,  -32256D0,       0D0 &
    ,    -144D0,   1440D0,   -6720D0,   20160D0,  -50400D0 &
    ,     240D0,  -2304D0,   10080D0,  -26880D0,   50400D0 &
    ,    -720D0,   6720D0,  -28224D0,   70560D0, -117600D0 &
    ,    5040D0, -46080D0,  188160D0, -451584D0,  705600D0/)&
    , (/5,9/));

contains 


SUBROUTINE numeric_deriv_test_all();  implicit none; 
  call dbg_info("-->numeric_deriv_test()"); 
  call test_numeric_deriv();
end SUBROUTINE

SUBROUTINE test_numeric_deriv();  implicit none; 
  integer :: nv;
  real(R14) :: minv, maxv, res, MAX_DRV_ERR = 1.d-18;
  real(R14), dimension(200) :: px, py, py2, pdy, pf;      
  real(R14), dimension(3) :: coeff, dc;      
  real(R14), dimension(6) :: c6, dc6;      
  real(R14), dimension(2) :: c2, dc2;
  type(VEC_T) :: vx, vy, vy2;
  call dbg_info("-->new_func_drv_test()"); 

  nv = 9;  minv = 0.; maxv = 1.;
  call calc_step_grid(px, 1, nv, minv, maxv);
  call assert_max_zero("9.px(1)=minv", px(1) - minv);
  call assert_max_zero("9.px(2)=minv", px(2) - (maxv+minv)/8.);
  call assert_max_zero("9.px(nv)=maxv", px(nv) - maxv);
  
  call setup_vec(vx, px, 1, 9);
  call calc_step_grid3(vx, minv, maxv);
  call assert_max_zero("9.px(1)=minv", px(1) - minv);
  call assert_max_zero("9.px(2)=minv", px(2) - (maxv+minv)/8.);
  call assert_max_zero("9.px(nv)=maxv", px(nv) - maxv);

  coeff = (/1, 0, 0/);  ! y=1
  call calc_func_polynom(py, px, coeff);  
  call calc_func_deriv(pdy, px, py);
  dc = (/0, 0, 0/);  ! y=0
  call calc_func_polynom(pf, px, dc);  
  call assert_max_diff_vec("pdy(1)", pdy, pf, MAX_DRV_ERR);

  coeff = (/D1, D1, D0/);  ! y=1+x
!  py => new_vec_polynom(px, coeff);  
  call calc_func_polynom(py, px, coeff);  
!  pdy => new_func_drv(px, py);
  call calc_func_deriv(pdy, px, py);
  res = D1;
  call assert_max_diff("pdy(1)", pdy(1), res, MAX_DRV_ERR);
  call assert_max_diff("pdy(2)", pdy(2), res, MAX_DRV_ERR);
  call assert_max_diff("pdy(3)", pdy(3), res, MAX_DRV_ERR);
  call assert_max_diff("pdy(4)", pdy(4), res, MAX_DRV_ERR);
  call assert_max_diff("pdy(5)", pdy(5), res, MAX_DRV_ERR);
  call assert_max_diff("pdy(6)", pdy(6), res, MAX_DRV_ERR);
  call assert_max_diff("pdy(7)", pdy(7), res, MAX_DRV_ERR);
  call assert_max_diff("pdy(8)", pdy(8), res, MAX_DRV_ERR);
  call assert_max_diff("pdy(9)", pdy(9), res, MAX_DRV_ERR);

  coeff = (/D0, D0, D05/);  
!  py => new_vec_polynom(px, coeff);  
  call calc_func_polynom(py, px, coeff);  
!  pdy => new_func_drv(px, py);
  call calc_func_deriv(pdy, px, py);
  dc = (/D0, D1, D0/);  
  pf => new_vec_polynom(px, dc);  
  call assert_max_diff_vec("y=0.5*x^2", pdy, pf, MAX_DRV_ERR);

  coeff = (/D0, D0, -D05/);  
!  py => new_vec_polynom(px, coeff);  
  call calc_func_polynom(py, px, coeff);  
  pdy => new_func_drv(px, py);
  dc = (/D0, -D1, D0/); 
  pf => new_vec_polynom(px, dc);  
  call assert_max_diff_vec("y=-0.5*x^2", pdy, pf, MAX_DRV_ERR);

  c6 = (/-D01, -D05, -D05, D0, D0, D0/);  
!  py => new_vec_polynom(px, c6);  
  call calc_func_polynom(py, px, c6);  
  pdy => new_func_drv(px, py);
  dc6 = (/-D05, -D1, D0, D0, D0, D0/);  
  pf => new_vec_polynom(px, dc6);  
  call assert_max_diff_vec("y=-0.1-0.5*x-0.5*x^2", pdy, pf, ERR14);

  c6 = (/-D02, D05, -D05, D2, -1D0/4D0, D0/);  
!  py => new_vec_polynom(px, c6);  
  call calc_func_polynom(py, px, c6);  
  pdy => new_func_drv(px, py);
  dc6 = (/D05, -D1, D6, -D1, D0, D0/);  
!  pf => new_vec_polynom(px, dc6);  
  call calc_func_polynom(pf, px, coeff);  
  call assert_max_diff_vec("y=...x^4/4", pdy, pf, ERR14);

!  py => new_vec_polynom(px, (/D0, D2/));  ! y=2x
  call calc_func_polynom(py, px, (/D0, D2/));  
  py2 => new_func_exp(py);  ! y=exp(2x)
  pdy => new_func_drv(px, py2);  ! y'=[exp(2x)]'=2exp(2x)
  pf => new_vec_from(py2);  
  pf = py2 * 2;  
  call assert_max_diff_vec("y=exp(2x)", pdy, pf, 2 * ERR5);

  ! NEW px
  nv = 101;  minv = -D1; maxv = D1;
  call del_vec(px);
!  px => new_step_grid(minv, maxv, vsize);
  px => new_vec(nv);  
  call calc_step_grid(px, 1, nv, minv, maxv);
!  py => new_vec_polynom(px, (/D0, D2/));  ! y=2x
  call calc_func_polynom(py, px, (/D0, D2/));  
  py2 => new_func_exp(py);  ! y=exp(2x)
  pdy => new_func_drv(px, py2);  ! y'=[exp(2x)]'=2exp(2x)
  pf => new_vec_from(py2);  
  pf = py2 * 2;  
  call assert_max_diff_vec("y=exp(2x)", pdy, pf, ERR11);

  c6 = (/-D02, D05, -D05, D2, -1D0/4D0, D0/);  
!  py => new_vec_polynom(px, c6);  
  call calc_func_polynom(py, px, c6);  
  pdy => new_func_drv(px, py);
  dc6 = (/D05, -D1, D6, -D1, D0, D0/);  
!  pf => new_vec_polynom(px, dc6);  
  call calc_func_polynom(pf, px, dc6);  
  call assert_max_diff_vec("y=...x^4/4", pdy, pf, ERR12);
end SUBROUTINE


SUBROUTINE calc_func_deriv(res, px, py);    implicit none;  
  real(R14), intent(OUT) :: res(:);      
  real(R14), intent(IN) :: px(:), py(:);      
  call load_deriv_p9(res, px, py);
end SUBROUTINE

! 9-point derivative
! NOTE: this func fast but could be made faster if needed.
! Not tested for dimension(J:J2), where J != 1
SUBROUTINE load_deriv_p9(res, px, py);    implicit none;  
  real(R14), intent(OUT) :: res(:);      
  real(R14), intent(IN) :: px(:), py(:);      
  real(R14) stx, h2; ! step x
  integer xn, mini, maxi, i, k, fromi;
  call dbg_info("-->func_drv_p9()", FUNC_DRV_TRACE_ENTRY);
  xn = SIZE(px, 1);  mini = LBOUND(px, 1); maxi = UBOUND(px, 1);
  call assert_true("xn >= 9", xn >= 9);
  call assert_true("xn == SIZE(py, 1)", xn == SIZE(py, 1));
  call assert_true("xn == SIZE(res, 1)", xn == SIZE(res, 1));
  call assert_true("mini == LBOUND(py, 1)", mini == LBOUND(py, 1));
  call assert_true("mini == LBOUND(res, 1)", mini == LBOUND(res, 1));
  call assert_step_grid_4(px);
  stx = px(mini+1) - px(mini);
  h2 = D05 / (20160D0 * stx);
 
  call dbg_vec("px=", px);
  call dbg_vec("py=", py);
  fromi = mini;
  do k = 1, 4 ! first 4
    i = fromi + k - 1;
    res(i) = h2 * calc_drv_p9(k, fromi, py);
    !call dbg_int("i=", i);
    !call dbg_real("pdy(i)=", pdy(i));
  end do
  k = 5;
  do i = mini+4, maxi-4 ! middle
    fromi = i - k + 1;    ! i = fromi + k - 1;
    res(i) = h2 * calc_drv_p9(k, fromi, py);
    !call dbg_int("i=", i);
    !call dbg_real("pdy(i)=", pdy(i));
  end do
  i = maxi-8;  
  do k = 6, 9 ! last 4
    i = fromi + k - 1;
    res(i) = h2 * calc_drv_p9(k, fromi, py);
    !call dbg_int("i=", (i+k-1));
    !call dbg_real("pdy(i)=", pdy(i));
  end do
  
  return;
end SUBROUTINE

real(R14) function calc_drv_p9(k, fromi, py) result(res);    implicit none;  
  real(R14), intent(IN) :: py(:);      
  integer midi, k, fromi, mk;  
  logical :: trace;
  trace = CACL_DRV_P9_TRACE;
  !call dbg_int("k=", k, trace);
  !call dbg_int("fromi=", fromi, trace);
  midi = fromi + 4;   !call dbg_int("midi=", midi, trace);
  mk = 10 - k; !mirror k

  !call dbg_real("DRV_P9(5, k)=", DRV_P9(5, k), trace);  
  res = DRV_P9(5, k) * py(midi); ! middle
  !call dbg_real("res=", res, trace);  

  !call dbg_real("  DRV_P9(4, k)=", DRV_P9(4, k), trace);  
  !call dbg_real("DRV_P9(4, mk)=", DRV_P9(4, mk), trace);  
  res = res + DRV_P9(4,k)*py(midi-1) - DRV_P9(4,mk)*py(midi+1);
  !call dbg_real("res=", res, trace);  

  !call dbg_real("  DRV_P9(3, k)=", DRV_P9(3, k), trace);  
  !call dbg_real("DRV_P9(3, mk)=", DRV_P9(3, mk), trace);  
  res = res + DRV_P9(3,k)*py(midi-2) - DRV_P9(3,mk)*py(midi+2);
  !call dbg_real("res=", res, trace);  

  !call dbg_real("  DRV_P9(2, k)=", DRV_P9(2, k), trace);  
  !call dbg_real("DRV_P9(2, mk)=", DRV_P9(2, mk), trace);  
  res = res + DRV_P9(2,k)*py(midi-3) - DRV_P9(2,mk)*py(midi+3);
  !call dbg_real("res=", res, trace);  

  res = res + DRV_P9(1,k)*py(midi-4) - DRV_P9(1,mk)*py(midi+4);
  !call dbg_real("res=", res, trace);  
  return;
end function

end module 
